#pragma once

#pragma warning(disable : 4799)
#include <xmmintrin.h>
#include <fvec.h>
#include <math.h>
#define PREFETCH(x) _mm_prefetch( (char*)(x), _MM_HINT_T0 );

typedef __m128	VectorRegister;
#define SHUFFLEMASK(A0,A1,B2,B3) ( (A0) | ((A1)<<2) | ((B2)<<4) | ((B3)<<6) )
__forceinline VectorRegister MakeVectorRegister( DWORD X, DWORD Y, DWORD Z, DWORD W )
{
	union { VectorRegister v; DWORD f[4]; } Tmp;
	Tmp.f[0] = X;
	Tmp.f[1] = Y;
	Tmp.f[2] = Z;
	Tmp.f[3] = W;
	return Tmp.v;
}
__forceinline VectorRegister MakeVectorRegister( float X, float Y, float Z, float W )
{
	return _mm_setr_ps( X, Y, Z, W );
}
static const VectorRegister SSE_ONE = MakeVectorRegister( 1.0f, 1.0f, 1.0f, 1.0f );
static const VectorRegister SSE_XYZ_MASK = MakeVectorRegister( (DWORD)0xffffffff, (DWORD)0xffffffff, (DWORD)0xffffffff, (DWORD)0x00000000 );

#define SIGN_BIT (~(1 << 31))
static const VectorRegister SSE_SIGN_MASK = MakeVectorRegister( (DWORD)SIGN_BIT, (DWORD)SIGN_BIT, (DWORD)SIGN_BIT, (DWORD)SIGN_BIT );

#define VectorZero()					_mm_setzero_ps()
#define VectorOne()						SSE_ONE
#define VectorLoad( Ptr )				_mm_loadu_ps( (float*)(Ptr) )
#define VectorLoadFloat3( Ptr )			_mm_loadu_ps( (float*)(Ptr) )
#define VectorLoadFloat3_W0( Ptr )		_mm_and_ps( _mm_loadu_ps((float*)(Ptr)), SSE_XYZ_MASK )
__forceinline VectorRegister VectorLoadFloat3_W1( const void* Ptr )
{
	// Vec = (Ptr[0], Ptr[1], Ptr[2], undefined)
	VectorRegister Vec = _mm_loadu_ps((const float*)Ptr);

	// Temp = (Ptr[2], undefined, 1.0f, 1.0f)
	VectorRegister Temp = _mm_movehl_ps( VectorOne(), Vec );

	// Return (Ptr[0], Ptr[1], Ptr[2], 1.0f)
	return _mm_shuffle_ps( Vec, Temp, SHUFFLEMASK(0,1,0,3) );
}
#define VectorLoadAligned( Ptr )		_mm_load_ps( (float*)(Ptr) )
#define VectorLoadFloat1( Ptr )			_mm_load1_ps( (float*)(Ptr) )
#define VectorSetFloat3( X, Y, Z )		MakeVectorRegister( X, Y, Z, 0.0f )
#define VectorSet( X, Y, Z, W )			MakeVectorRegister( X, Y, Z, W )
#define VectorStoreAligned( Vec, Ptr )	_mm_store_ps( (float*)(Ptr), Vec )
#define VectorStore( Vec, Ptr )			_mm_storeu_ps( (float*)(Ptr), Vec )
__forceinline void VectorStoreFloat3( const VectorRegister& Vec, void* Ptr )
{
	union { VectorRegister v; float f[4]; } Tmp;
	Tmp.v = Vec;
	float* FloatPtr = (float*)(Ptr);
	FloatPtr[0] = Tmp.f[0];
	FloatPtr[1] = Tmp.f[1];
	FloatPtr[2] = Tmp.f[2];
}
#define VectorStoreFloat1( Vec, Ptr )	_mm_store_ss((float*)(Ptr), Vec)
#define VectorReplicate( Vec, ElementIndex )	_mm_shuffle_ps( Vec, Vec, SHUFFLEMASK(ElementIndex,ElementIndex,ElementIndex,ElementIndex) )
#define VectorAbs( Vec )				_mm_and_ps(Vec, SSE_SIGN_MASK)
#define VectorNegate( Vec )				_mm_sub_ps(_mm_setzero_ps(),Vec)
#define VectorAdd( Vec1, Vec2 )			_mm_add_ps( Vec1, Vec2 )
#define VectorSubtract( Vec1, Vec2 )	_mm_sub_ps( Vec1, Vec2 )
#define VectorMultiply( Vec1, Vec2 )	_mm_mul_ps( Vec1, Vec2 )
#define VectorMultiplyAdd( Vec1, Vec2, Vec3 )	_mm_add_ps( _mm_mul_ps(Vec1, Vec2), Vec3 )
__forceinline VectorRegister VectorDot3( const VectorRegister& Vec1, const VectorRegister& Vec2 )
{
	VectorRegister Temp = VectorMultiply( Vec1, Vec2 );
	return VectorAdd( VectorReplicate(Temp,0), VectorAdd( VectorReplicate(Temp,1), VectorReplicate(Temp,2) ) );
}
__forceinline VectorRegister VectorDot4( const VectorRegister& Vec1, const VectorRegister& Vec2 )
{
	VectorRegister Temp1, Temp2;
	Temp1 = VectorMultiply( Vec1, Vec2 );
	Temp2 = _mm_shuffle_ps( Temp1, Temp1, SHUFFLEMASK(2,3,0,1) );	// (Z,W,X,Y).
	Temp1 = VectorAdd( Temp1, Temp2 );								// (X*X + Z*Z, Y*Y + W*W, Z*Z + X*X, W*W + Y*Y)
	Temp2 = _mm_shuffle_ps( Temp1, Temp1, SHUFFLEMASK(1,2,3,0) );	// Rotate left 4 bytes (Y,Z,W,X).
	return VectorAdd( Temp1, Temp2 );								// (X*X + Z*Z + Y*Y + W*W, Y*Y + W*W + Z*Z + X*X, Z*Z + X*X + W*W + Y*Y, W*W + Y*Y + X*X + Z*Z)
}
__forceinline VectorRegister VectorCross( const VectorRegister& Vec1, const VectorRegister& Vec2 )
{
	VectorRegister A_YZXW = _mm_shuffle_ps( Vec1, Vec1, SHUFFLEMASK(1,2,0,3) );
	VectorRegister B_ZXYW = _mm_shuffle_ps( Vec2, Vec2, SHUFFLEMASK(2,0,1,3) );
	VectorRegister A_ZXYW = _mm_shuffle_ps( Vec1, Vec1, SHUFFLEMASK(2,0,1,3) );
	VectorRegister B_YZXW = _mm_shuffle_ps( Vec2, Vec2, SHUFFLEMASK(1,2,0,3) );
	return VectorSubtract( VectorMultiply(A_YZXW,B_ZXYW), VectorMultiply(A_ZXYW, B_YZXW) );
}
__forceinline VectorRegister VectorPow( const VectorRegister& Base, const VectorRegister& Exponent )
{
	//@TODO: Optimize
	union { VectorRegister v; float f[4]; } B, E;
	B.v = Base;
	E.v = Exponent;
	return _mm_setr_ps( powf(B.f[0], E.f[0]), powf(B.f[1], E.f[1]), powf(B.f[2], E.f[2]), powf(B.f[3], E.f[3]) );
}
__forceinline void VectorMatrixMultiply( void *Result, const void* Matrix1, const void* Matrix2 )
{
	const VectorRegister *A	= (const VectorRegister *) Matrix1;
	const VectorRegister *B	= (const VectorRegister *) Matrix2;
	VectorRegister *R		= (VectorRegister *) Result;
	VectorRegister Temp, R0, R1, R2, R3;

	// First row of result (Matrix1[0] * Matrix2).
	Temp	= VectorMultiply( VectorReplicate( A[0], 0 ), B[0] );
	Temp	= VectorMultiplyAdd( VectorReplicate( A[0], 1 ), B[1], Temp );
	Temp	= VectorMultiplyAdd( VectorReplicate( A[0], 2 ), B[2], Temp );
	R0		= VectorMultiplyAdd( VectorReplicate( A[0], 3 ), B[3], Temp );

	// Second row of result (Matrix1[1] * Matrix2).
	Temp	= VectorMultiply( VectorReplicate( A[1], 0 ), B[0] );
	Temp	= VectorMultiplyAdd( VectorReplicate( A[1], 1 ), B[1], Temp );
	Temp	= VectorMultiplyAdd( VectorReplicate( A[1], 2 ), B[2], Temp );
	R1		= VectorMultiplyAdd( VectorReplicate( A[1], 3 ), B[3], Temp );

	// Third row of result (Matrix1[2] * Matrix2).
	Temp	= VectorMultiply( VectorReplicate( A[2], 0 ), B[0] );
	Temp	= VectorMultiplyAdd( VectorReplicate( A[2], 1 ), B[1], Temp );
	Temp	= VectorMultiplyAdd( VectorReplicate( A[2], 2 ), B[2], Temp );
	R2		= VectorMultiplyAdd( VectorReplicate( A[2], 3 ), B[3], Temp );

	// Fourth row of result (Matrix1[3] * Matrix2).
	Temp	= VectorMultiply( VectorReplicate( A[3], 0 ), B[0] );
	Temp	= VectorMultiplyAdd( VectorReplicate( A[3], 1 ), B[1], Temp );
	Temp	= VectorMultiplyAdd( VectorReplicate( A[3], 2 ), B[2], Temp );
	R3		= VectorMultiplyAdd( VectorReplicate( A[3], 3 ), B[3], Temp );

	// Store result
	R[0] = R0;
	R[1] = R1;
	R[2] = R2;
	R[3] = R3;
}
#define VectorMin( Vec1, Vec2 )			_mm_min_ps( Vec1, Vec2 )
#define VectorMax( Vec1, Vec2 )			_mm_max_ps( Vec1, Vec2 )
#define VectorSwizzle( Vec, X, Y, Z, W )	_mm_shuffle_ps( Vec, Vec, SHUFFLEMASK(X,Y,Z,W) )
#define VectorLoadByte4( Ptr )			_mm_cvtpu8_ps( *((__m64*)Ptr) )
__forceinline VectorRegister VectorLoadByte4Reverse( void* Ptr )
{
	VectorRegister Temp = _mm_cvtpu8_ps( *((__m64*)Ptr) );
	return _mm_shuffle_ps( Temp, Temp, SHUFFLEMASK(3,2,1,0) );
}
__forceinline void VectorStoreByte4( const VectorRegister& Vec, void* Ptr )
{
	__m64 NewVec = _mm_packs_pu16( _mm_cvtps_pi16(Vec), _mm_setzero_si64() );
	*((DWORD*)Ptr) = NewVec.m64_u32[0];
}
#define VectoryAnyGreaterThan( Vec1, Vec2 )		move_mask( _mm_cmpgt_ps(Vec1, Vec2) )
#define VectorResetFloatRegisters()		_mm_empty()
#define VectorGetControlRegister()		_mm_getcsr()
#define	VectorSetControlRegister(ControlStatus) _mm_setcsr( ControlStatus )
#define VECTOR_ROUND_TOWARD_ZERO		_MM_ROUND_TOWARD_ZERO
